/*
 * @Date: 2020-09-03 22:51:11
 * @LastEditors: Tonser
 * @LastEditTime: 2020-10-04 21:25:19
 * @FilePath: /swarm_ws/src/px4_swarm_control/include/MsgLink/UWBManager.h
 * @Description: uwb manager
 */
#ifndef INCLUDE_UWB_MANAGER
#define INCLUDE_UWB_MANAGER

#include <cstdint>
#include <map>
#include <set>
#include <stdio.h>
#include <string>
#include <vector>
#include <unistd.h>
#include "../Utils/Serial_Utils.h"
#include "SwarmMessage.h"

#define MAX_NUM 5

DATA_FRAM RemoteNodeDistanceFrame {
    uint8_t role;
    uint8_t id;
    int32_t distance : 24;
    uint8_t fp_rssi;
    uint8_t rx_rssi;
    uint32_t rx_lps_systime;
    char reserved[2];
};

DATA_FRAM RemoteNodeLittleDistanceFrame {
    uint8_t role;
    uint8_t id;
    int32_t distance : 24;
    uint8_t fp_rssi;
    uint8_t rx_rssi;
};

DATA_FRAM RemoteNodeBroadCastHeader {
    uint8_t role;
    uint8_t id;
    uint16_t data_length;
};


DATA_FRAM NodeRecvNodeNumsFrame {
    char head[2];
    uint16_t frame_length;
    uint8_t role_table;
    uint8_t id;
    char reserved[4];
    uint8_t recv_node_nums;
};


DATA_FRAM NodeValidNodeQuantityDistanceFrame {
    char head[2];
    uint16_t frame_length;
    uint8_t role_table;
    uint8_t id;
    uint32_t lps_time;
    char reserved[108];
    uint8_t valid_node_quantity;
};

DATA_FRAM NodeValidNodeQuantityLittleDistanceFrame {
    char head[2];
    uint16_t frame_length;
    uint8_t role_table;
    uint8_t id;
    uint32_t local_time;
    uint32_t lps_time;
    char reserved[4];
    uint16_t voltage;
    uint8_t valid_node_quantity;
};

struct RemoteNodeInfo {
    double distance = -1;
    uint32_t dis_time;
    double fp_rssi = 0;
    double rx_rssi = 0;
    bool active = false;
    int id = 0;
    int role = 0;
    std::string to_str() {
        char str[100] = {0};
        sprintf(str, " id:%d role %d dis:%f fprssi:%f rxrssi:%f active:%d", id,
                role, distance, fp_rssi, rx_rssi, active);
        std::string _str(str);
        return str;
    }
};

class UWBManager {
public:
    // 解析状态
    enum {
        WAIT_FOR_HEADER,
        WAIT_FOR_NODE_DETAIL,
        WAIT_FOR_NODE_CHECKSUM,
    };

    // NodeFrame 
    enum {
        NODE_BROAD_CAST_FRAME, // [head : recv node nums]  [detail : broadcast data]
        NODE_FRAME1,
        NODE_DISTANCE_FRAME, //    [head : valid nodes ]    [detail : distance]
        NODE_LITTLE_DISTANCE_FRAME
    };
    UWBManager()=default;
    UWBManager(std::string serial_name, int baudrate,
                  bool enable_debug_output = false);
    void setUWBManager(std::string serial_name, int baudrate,
                  bool enable_debug_output = false);
    std::vector<uint8_t> buf;
    int self_id;
    int sys_time = -1;
    int valid_node_quantity = 0;

    uint8_t recv_type_now = -1;

    int64_t sum_check = 0;
    void readAndParse();

    void sendBroadcastData(std::vector<uint8_t> msg);

    bool uwb_ok = false;

    void close_port() {
        printf("Closing port\n");
        close(serial_fd);
    }

protected:
    void reset_checksum();
    void deleteFirstNBytesInBuf(int _len);
    int parseData();

    bool enable_debug_output;

    virtual void onBroadcastDataRecv(int _id, Buffer _msg);
    void onNodeDataRecv(RemoteNodeDistanceFrame nf);
    void onNodeDataRecv(RemoteNodeLittleDistanceFrame nf);

    virtual void onNodeDataUpdated();
    std::map<int, RemoteNodeInfo> nodes_info;

    uint8_t frameType();

private:
    int serial_fd;
    void configurePort(int baudrate);
    bool openPort(std::string serial_name, int baudrate);
    uint8_t readBytesFromSerial();
    int serialAvailableBytes();
    int read_status = WAIT_FOR_HEADER;
    int read_wait_remote_node_num = 0;
    std::set<int> active_node_set;
    bool isNodeMsgHeader();
    void parseHeader();
    virtual void onSystemTimeUpdate() {};
    int parseValidNodeQuantityDistanceFrame();
    int parseValidNodeQuantityLittleDistanceFrame();
    int parseRecvNodeNumsFrame();
    bool parseRemoteNodeDistanceFrame();
    bool parseRemoteNodeLittleDistanceFrame();
    bool parseRemoteNodeBroadCastFrame();
    void serialWrite(uint8_t *data, int len);
};

#endif